/*
 __  __  __    ____    ____    ___    ___    ___   
/\ \/\ \/\ \  /',__\  /',__\  /'___\ /'___\ /'___\ 
\ \ \_/ \_/ \/\__, `\/\__, `\/\ \__//\ \__//\ \__/ 
 \ \___x___/'\/\____/\/\____/\ \____\ \____\ \____\
  \/__//__/   \/___/  \/___/  \/____/\/____/\/____/
                                                   
  */


//2010-4-11

#define STATE_FORWARD 1
#define STATE_BACKWARD 2
#define STATE_LEFT 3
#define STATE_RIGHT 4



#define GENERAL_SPEED 15
#define GENERAL_TURNSPEED 100
#include "wssccc.h"
//#include "arct.h"



#define RTC_LIMIT 3
#define FI_LIMIT 3
#define LTC_LIMIT 150
#define ARCC_LIMIT 50



     
void mazeLoop(){
	while(1){
			 
		load_state();
		
		detectSensors();
		


		//handles
		
		if(!impact(FORE_SENSOR)&&impact(LEFT_SENSOR))
		{
			forward(2);
			continue;
		}
		
		if((!impact(FORE_SENSOR))&&(!impact(LEFT_SENSOR))){
			
						
			++g_LTC;
			if(g_LTC>LTC_LIMIT){
			    //stop();//test

			    stop();  //test
				
			    //forward(5);
			    g_LTC=0;
			    continue;

			}
			
			    
			   
			//if(impact(RIGHT_SENSOR)){
			//	o_turnLeft(GENERAL_TURNSPEED);
			//	continue;
			//}

			
			


			if(g_state!=STATE_FORWARD){

				//forward(2);
				g_state=STATE_LEFT;
			}else{
				/*stop();
				delay_ms(10);

				PORTD=0b00010000;
				delay_ms(25);//adjust later
				PORTD=0x00;

				arcTurn_begin(R18);   //reserved
				delay_ms(20);

				arcTurn_end();
				*/
			
				
				//pulse stop
				PORTD=0x00;
				PORTD=0b00010000;
				delay_ms(50);
				
				//load_state();
				
				
				PORTD=0x00;
						
				//------
				
				
				
				savePCstate();
				
				PORTD=0b01000000;
				
				g_ARCC=0;   //for arc turn
				
				//arcTurn_begin(65);
				
				//
				while(g_ARCC<ARCC_LIMIT){
									  if(!checkPCstate())break;
									  
									  ++g_ARCC;   //for arc turn 
									  					  					  
									  delay_ms(10);
									  //light_on(0);
									  //light_on(1);
									  //light_on(2);
									  }
				

			}

			continue;

		}else{
		    g_LTC=0;
		}
		
		
		if(impact(LEFT_SENSOR)&&impact(FORE_SENSOR)){
			//if(g_state==STATE_FORWARD);//stop();

			++g_RTC;
			
			if(g_RTC>RTC_LIMIT){
				//o_turnRight(7);
				forward(10);
				g_RTC=0;

			}else{			
			d_turnRight(10);
			forward(10);
			//turnRight(GENERAL_TURNSPEED);
			}

			continue;
		}else{
			g_RTC=0;
		}

		//&&(!impact(RIGHT_SENSOR))
		if((impact(FORE_SENSOR))&&(!impact(LEFT_SENSOR))){
			++g_foreimpact;
			if(g_state==STATE_FORWARD)//stop();

			turnRight(GENERAL_TURNSPEED);
			if(g_foreimpact>FI_LIMIT){
				backward(3);
				g_foreimpact=0;
			}

			continue;
		}else{
			g_foreimpact=0;
		}

		
/*
		if(impact(FORE_SENSOR)&&impact(RIGHT_SENSOR)){

			if(g_state==STATE_FORWARD);//stop();

			turnLeft(GENERAL_TURNSPEED);
			forward(2);
			continue;
		}

		
*/


		


		


	}
}


void main(){

	init_devices();

	g_foreimpact=0;
	g_RTC=0;
	g_state=0;
	g_LTC=0;
	g_pcstate=0;
	g_ARCC=0;
	

	//turnLeft(15);		
	//while(1)detectSensors();
	waitSound(300);
	//while(1)forward(10,50);

			 
			 
	mazeLoop();




}
